#include "arm.h"
#include "console.h"
#include "reentrant_lock.h"

static uint64_t owner_id() {
    return (arm_cpuid() << 1) | 1;
}

void reentrant_acquire(reentrant_lock_t *lk) {
    uint64_t self = owner_id();

    if (lk->owner == self)
        lk->count++;
    else {
        union {
            uint64_t content;
            reentrant_lock_t object;
        } expected, value;

        expected.content = 0;
        value.object.owner = self;
        value.object.count = 1;

        while (!__atomic_compare_exchange_n(
            (uint64_t*) lk, &expected.content, value.content,
            0, __ATOMIC_ACQUIRE, __ATOMIC_ACQUIRE
        )) {
            expected.content = 0;
        }
    }
}

void reentrant_release(reentrant_lock_t *lk) {
    if (lk->count == 1) {
        uint64_t self = owner_id();

        union {
            uint64_t content;
            reentrant_lock_t object;
        } expected, value;

        expected.object.owner = self;
        expected.object.count = 1;
        value.content = 0;

        assert(__atomic_compare_exchange_n(
            (uint64_t*) lk, &expected.content, value.content,
            0, __ATOMIC_RELEASE, __ATOMIC_ACQUIRE
        ));
    } else
        lk->count--;
}